{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# R36 DERIVATION\n",
    "\n",
    "**We know the following from our geometry:**\n",
    "```\n",
    "1. R0u = R06 * R6g * Rgu \n",
    "2. R03 = R03 * R36\n",
    "3. R6g = I\n",
    "4. R0u = rotz(yaw) * roty(pitch) * rotx(roll)\n",
    "5. Rgu = rotz(pi) * roty(-pi/2)\n",
    "```\n",
    "\n",
    "**For any matrix recall the following properties:**\n",
    "```\n",
    "1. R = I * R = R * I\n",
    "2. (A * B) * C = A * (B * C)\n",
    "3. if A = B then A * C = B * C and vice versa\n",
    "4. if A = B then C * A = C * C and vice versa\n",
    "5. R * R.inverse() = R.inverse() * R = I \n",
    "```\n",
    "\n",
    "**Note that for rotation matrices**\n",
    "```\n",
    "R.inverse() = R.T \n",
    "Therefore: R.T * R = R * R.T = I \n",
    "```\n",
    "\n",
    "**From the discussion above, you can do the following:** \n",
    "- _Subsitution_  \n",
    "- - `R0u = (R03 * R36) * I * Rgu`\n",
    "- _Multiply both sides by `Rgu.inverse() = Rgu.T` at the right_\n",
    "- - `R0u * Rgu.T = R03 * R36 `\n",
    "- _Multiply both sides by `R03.inverse() = R03.T` at the left_ \n",
    "- - `R03.T * R0u * Rgu.T = R36 `\n",
    "\n",
    "**We conclude then that:**\n",
    "```\n",
    "R36 = R03.T * R0u * Rgu.T\n",
    "R36 = R03.T * R0g\n",
    "where:\n",
    "R0u = rotz(yaw) * roty(pitch) * rotx(roll)\n",
    "Rgu = rotz(pi) * roty(-pi/2)\n",
    "```\n",
    "`\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import symbols, cos, sin, pi, simplify, trigsimp, expand_trig, pprint, sqrt, atan2\n",
    "from sympy.matrices import Matrix\n",
    "\n",
    "def rotx(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [1., 0., 0.],\n",
    "    [0., cq,-sq],\n",
    "    [0., sq, cq]\n",
    "  ])\n",
    "    \n",
    "  return r\n",
    "\n",
    "\n",
    "def roty(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [ cq, 0., sq],\n",
    "    [ 0., 1., 0.],\n",
    "    [-sq, 0., cq]\n",
    "  ])\n",
    "    \n",
    "  return r\n",
    "\n",
    "\n",
    "def rotz(q):\n",
    "\n",
    "  sq, cq = sin(q), cos(q)\n",
    "\n",
    "  r = Matrix([\n",
    "    [cq,-sq, 0.],\n",
    "    [sq, cq, 0.],\n",
    "    [0., 0., 1.]\n",
    "  ])\n",
    "    \n",
    "  return r"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡sin(q₂ + q₃)⋅cos(q₁)  sin(q₁)⋅sin(q₂ + q₃)  cos(q₂ + q₃) ⎤\n",
      "⎢                                                         ⎥\n",
      "⎢cos(q₁)⋅cos(q₂ + q₃)  sin(q₁)⋅cos(q₂ + q₃)  -sin(q₂ + q₃)⎥\n",
      "⎢                                                         ⎥\n",
      "⎣      -sin(q₁)              cos(q₁)               0      ⎦\n"
     ]
    }
   ],
   "source": [
    "q1, q2, q3, q4, q5, q6= symbols('q1:7')\n",
    "\n",
    "R03 = Matrix([\n",
    "[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],\n",
    "[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],\n",
    "[        cos(q2 + q3),        -sin(q2 + q3),        0]])\n",
    "\n",
    "R03T = R03.T\n",
    "pprint(R03T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 56,
   "metadata": {},
   "outputs": [],
   "source": [
    "R36 = Matrix([[-sin(q4)*sin(q6) + cos(q4)*cos(q5)*cos(q6), -sin(q4)*cos(q6) - sin(q6)*cos(q4)*cos(q5), -sin(q5)*cos(q4)],\n",
    "  [                           sin(q5)*cos(q6),                           -sin(q5)*sin(q6),          cos(q5)],\n",
    "  [-sin(q4)*cos(q5)*cos(q6) - sin(q6)*cos(q4),  sin(q4)*sin(q6)*cos(q5) - cos(q4)*cos(q6),  sin(q4)*sin(q5)]])\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡1.0⋅cos(α)⋅cos(β)  -1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α)  1.0⋅sin(α)⋅sin(\n",
      "⎢                                                                             \n",
      "⎢1.0⋅sin(α)⋅cos(β)  sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ)   sin(α)⋅sin(β)⋅c\n",
      "⎢                                                                             \n",
      "⎣   -1.0⋅sin(β)                 1.0⋅sin(γ)⋅cos(β)                         1.0⋅\n",
      "\n",
      "γ) + sin(β)⋅cos(α)⋅cos(γ)⎤\n",
      "                         ⎥\n",
      "os(γ) - 1.0⋅sin(γ)⋅cos(α)⎥\n",
      "                         ⎥\n",
      "cos(β)⋅cos(γ)            ⎦\n"
     ]
    }
   ],
   "source": [
    "alpha, beta, gamma = symbols('alpha beta gamma', real = True)\n",
    "R0u = rotz(alpha) * roty(beta) * rotx(gamma)\n",
    "pprint(R0u)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡0   0    1.0⎤\n",
      "⎢            ⎥\n",
      "⎢0  -1.0   0 ⎥\n",
      "⎢            ⎥\n",
      "⎣1   0     0 ⎦\n"
     ]
    }
   ],
   "source": [
    "RugT = (rotz(pi) * roty(-pi/2)).T\n",
    "pprint(RugT)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡(1.0⋅sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ))⋅sin(q₂ + q₃)⋅cos(q₁) + (sin(α)⋅sin\n",
      "⎢                                                                             \n",
      "⎢(1.0⋅sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ))⋅cos(q₁)⋅cos(q₂ + q₃) + (sin(α)⋅sin\n",
      "⎢                                                                             \n",
      "⎣                             -(1.0⋅sin(α)⋅sin(γ) + sin(β)⋅cos(α)⋅cos(γ))⋅sin(\n",
      "\n",
      "(β)⋅cos(γ) - 1.0⋅sin(γ)⋅cos(α))⋅sin(q₁)⋅sin(q₂ + q₃) + 1.0⋅cos(β)⋅cos(γ)⋅cos(q\n",
      "                                                                              \n",
      "(β)⋅cos(γ) - 1.0⋅sin(γ)⋅cos(α))⋅sin(q₁)⋅cos(q₂ + q₃) - 1.0⋅sin(q₂ + q₃)⋅cos(β)\n",
      "                                                                              \n",
      "q₁) + (sin(α)⋅sin(β)⋅cos(γ) - 1.0⋅sin(γ)⋅cos(α))⋅cos(q₁)                      \n",
      "\n",
      "₂ + q₃)  -1.0⋅(-1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α))⋅sin(q₂ + q₃)⋅cos(q₁)\n",
      "                                                                              \n",
      "⋅cos(γ)  -1.0⋅(-1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)⋅cos(α))⋅cos(q₁)⋅cos(q₂ + q₃)\n",
      "                                                                              \n",
      "                                       1.0⋅(-1.0⋅sin(α)⋅cos(γ) + sin(β)⋅sin(γ)\n",
      "\n",
      " - 1.0⋅(sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ))⋅sin(q₁)⋅sin(q₂ + q₃) - 1.0⋅s\n",
      "                                                                              \n",
      " - 1.0⋅(sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ))⋅sin(q₁)⋅cos(q₂ + q₃) + 1.0⋅s\n",
      "                                                                              \n",
      "⋅cos(α))⋅sin(q₁) - 1.0⋅(sin(α)⋅sin(β)⋅sin(γ) + 1.0⋅cos(α)⋅cos(γ))⋅cos(q₁)     \n",
      "\n",
      "in(γ)⋅cos(β)⋅cos(q₂ + q₃)  1.0⋅sin(α)⋅sin(q₁)⋅sin(q₂ + q₃)⋅cos(β) - 1.0⋅sin(β)\n",
      "                                                                              \n",
      "in(γ)⋅sin(q₂ + q₃)⋅cos(β)  1.0⋅sin(α)⋅sin(q₁)⋅cos(β)⋅cos(q₂ + q₃) + 1.0⋅sin(β)\n",
      "                                                                              \n",
      "                                                     1.0⋅sin(α)⋅cos(β)⋅cos(q₁)\n",
      "\n",
      "⋅cos(q₂ + q₃) + 1.0⋅sin(q₂ + q₃)⋅cos(α)⋅cos(β)⋅cos(q₁)⎤\n",
      "                                                      ⎥\n",
      "⋅sin(q₂ + q₃) + 1.0⋅cos(α)⋅cos(β)⋅cos(q₁)⋅cos(q₂ + q₃)⎥\n",
      "                                                      ⎥\n",
      " - 1.0⋅sin(q₁)⋅cos(α)⋅cos(β)                          ⎦\n"
     ]
    }
   ],
   "source": [
    "R36 = R03T * R0u * RugT\n",
    "pprint(R36)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡ 0.724598074610823   -0.686235503719083   0.0635489079820028⎤\n",
      "⎢                                                            ⎥\n",
      "⎢ 0.684428504026748    0.727345057020894   0.0502671950976013⎥\n",
      "⎢                                                            ⎥\n",
      "⎣-0.0807171180481332  0.00707117123884714  0.996711967115533 ⎦\n",
      "(0.756897144385541, 0.0808050247662215, 0.00709437915922838)\n"
     ]
    }
   ],
   "source": [
    "q1, q2, q3, q4, q5, q6= symbols('q1:7')\n",
    "\n",
    "R03 = Matrix([\n",
    "[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],\n",
    "[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],\n",
    "[        cos(q2 + q3),        -sin(q2 + q3),        0]])\n",
    "\n",
    "R03T = R03.T\n",
    "\n",
    "alpha, beta, gamma = symbols('alpha beta gamma')\n",
    "R0u = rotz(alpha) * roty(beta) * rotx(gamma)\n",
    "\n",
    "RugT = (rotz(pi) * roty(-pi/2)).T\n",
    "R36 = R03T * R0u * RugT\n",
    "\n",
    "\n",
    "roll, pitch, yaw = 0.366, -0.078, 2.561\n",
    "\n",
    "variables = {\n",
    "  q1: 1.01249, \n",
    "  q2: -0.2758, \n",
    "  q3: -0.11568,\n",
    "  alpha: yaw,\n",
    "  beta: pitch, \n",
    "  gamma: roll\n",
    "}\n",
    "\n",
    "R0g = R0u * Rug\n",
    "R36eval = R36.evalf(subs = variables)\n",
    "\n",
    "pprint(R36eval)\n",
    "\n",
    "print(get_spherical_ik(R36eval))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "True\n",
      "True\n"
     ]
    }
   ],
   "source": [
    "print(simplify(R03I) == R03.T)\n",
    "print(Rug == Rug.T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡ 0.724598074610823   -0.686235503719083   0.0635489079820028⎤\n",
      "⎢                                                            ⎥\n",
      "⎢ 0.684428504026748    0.727345057020894   0.0502671950976012⎥\n",
      "⎢                                                            ⎥\n",
      "⎣-0.0807171180481332  0.00707117123884723  0.996711967115533 ⎦\n"
     ]
    }
   ],
   "source": [
    "R03 = Matrix([\n",
    "[sin(q2 + q3)*cos(q1), cos(q1)*cos(q2 + q3), -sin(q1)],\n",
    "[sin(q1)*sin(q2 + q3), sin(q1)*cos(q2 + q3),  cos(q1)],\n",
    "[        cos(q2 + q3),        -sin(q2 + q3),        0]])\n",
    "\n",
    "roll, pitch, yaw = 0.366, -0.078, 2.561\n",
    "R0u = rotz(yaw)* roty(pitch) * rotx(roll)\n",
    "Rug = (rotz(pi) * roty(-pi/2)).T\n",
    "\n",
    "R0u.evalf(subs = {alpha: yaw, beta: pitch, gamma: roll})\n",
    "R36eval2 = R36.evalf(subs = {q1: 1.01249, q2: -0.2758, q3: -0.11568})\n",
    "pprint(R36eval2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "(1.63446868902482, 1.52050793847535, -0.815787839019348)\n",
      "Matrix([[0.724598074610823, -0.686235503719083, 0.0635489079820028], [0.684428504026748, 0.727345057020894, 0.0502671950976013], [-0.0807171180481332, 0.00707117123884714, 0.996711967115533]])\n"
     ]
    }
   ],
   "source": [
    "\n",
    "\n",
    "def ik(R):\n",
    "  r12, r13 = R[0,1], R[0,2]\n",
    "  r21, r22, r23 = R[1,0], R[1,1], R[1,2] \n",
    "  r32, r33 = R[2,1], R[2,2]\n",
    "  q5 = atan2(sqrt(r13**2 + r33**2), r23)\n",
    "  q4 = atan2(r33, -r13)\n",
    "  q6 = atan2(-r22, r21)\n",
    "  return q4.evalf(), q5.evalf(), q6.evalf()\n",
    "\n",
    "print(ik(R36eval))\n",
    "\n",
    "print(R36eval)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 60,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Matrix([[0.257143295038827, 0.488872082559650, -0.833595473062544], [0.259329420712765, 0.796053601157403, 0.546851822377060], [0.930927267496960, -0.356795110642117, 0.0779209320563015]])\n"
     ]
    }
   ],
   "source": [
    "R0g = rotz(yaw) * roty(pitch) * rotx(roll) * Rug\n",
    "print(R0g)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 66,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "⎡-0.202129925357168  -0.323618808899201  0.924345368248129⎤\n",
      "⎢                                                         ⎥\n",
      "⎢0.489672387232293   0.783986806638987   0.381556863649747⎥\n",
      "⎢                                                         ⎥\n",
      "⎣-0.848153551226034  0.529750463466212           0        ⎦\n",
      "⎡sin(q₂ + q₃)⋅cos(q₁)  sin(q₁)⋅sin(q₂ + q₃)  cos(q₂ + q₃) ⎤\n",
      "⎢                                                         ⎥\n",
      "⎢cos(q₁)⋅cos(q₂ + q₃)  sin(q₁)⋅cos(q₂ + q₃)  -sin(q₂ + q₃)⎥\n",
      "⎢                                                         ⎥\n",
      "⎣      -sin(q₁)              cos(q₁)               0      ⎦\n"
     ]
    }
   ],
   "source": [
    "R03T_eval =R03T.evalf(subs = { q1: 1.01249, q2: -0.2758, q3: -0.11568})\n",
    "pprint(R03T_eval)\n",
    "pprint(R03T)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.5.2"
  },
  "widgets": {
   "state": {},
   "version": "1.1.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
